#include "CWorker.h"
#include <QDebug>

CWorker::CWorker(QObject *parent) : QObject(parent)
{
}

CWorker::~CWorker()
{
    delete m_map_manager;
    m_map_manager = nullptr;

    if (m_rrt)
    {
        delete m_rrt;
        m_rrt = nullptr;
    }
}

/**
 * @brief CWorker::createObject
 */
void CWorker::createObject()
{
    qDebug("[CWorker] createObject enter\n");

    m_map_manager = new CGridMap(1000, 1000);

    m_rrt = new CPathPlanRRT(10.0, 0.1);
    m_mutex = std::make_unique<QMutex> ();

    connect(m_rrt, &CPathPlanRRT::signalDisplayRandomPoint, this, &CWorker::signalDisplayRandomPoint);
    connect(m_rrt, &CPathPlanRRT::signalDisplayTreeNode, this, &CWorker::signalDisplayTreeNode);
    connect(m_rrt, &CPathPlanRRT::signalDisplayOriginPath, this, &CWorker::signalDisplayOriginPath);

    qDebug("[CWorker] createObject exit\n");
}

void CWorker::slotUpdateGridMapSize(quint16 height, quint16 width)
{
    qDebug("[CWorker] updateGridMapSize enter, [%d, %d]\n", height, width);
    m_map_manager->initGridMap(height, width);
}

void CWorker::slotUpdateGridMapData(QVector<QPoint> data)
{
    m_map_manager->updateGridMapData(data);
}

void CWorker::slotResetGridMapData()
{
    m_map_manager->resetMap();
}

void CWorker::slotSetStartPoint(QPoint point)
{
    qDebug("[CWorker] slotSetStartPoint, [%d, %d]\n", point.x(), point.y());
    if (m_rrt != nullptr)
        m_rrt->setStartPoint(point);
}

void CWorker::slotSetDestPoint(QPoint point)
{
    qDebug("[CWorker] slotSetDestPoint, [%d, %d]\n", point.x(), point.y());
    if (m_rrt != nullptr)
        m_rrt->setDestPoint(point);
}

void CWorker::slotStartPlanning()
{
    qDebug("[CWorker] slotStartPlanning\n");
    m_rrt->updateMap(*m_map_manager);
    m_rrt->planning();
    qDebug("[CWorker] slotStartPlanning finish\n");
}

